from carla import Transform, Location, Rotation


class LidarUtil:

    def __init__(self, client, world, settings, traffic_manager):
        # ==================carla初始化======================
        self.client = client
        self.world = world
        self.settings = settings
        self.traffic_manager = traffic_manager

    # 将雷达的点云保存到队列
    def lidar_callback(self, sensor_data, sensor_queue, sensor_name):
        sensor_queue.put((sensor_data.frame, sensor_data.timestamp, sensor_name, sensor_data))

    def get_lidar_64(self, carla_car, transform, point_cloud_queue, lidar_list):
        # 获取carla中的雷达
        lidar_bp_2_1 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_2_2 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_2_3 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_1 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_2 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_4_3 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_1 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_2 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_6_3 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        lidar_bp_28 = self.world.get_blueprint_library().blueprint_library.find('sensor.lidar.ray_cast')
        # 设置传感器公共参数
        atmosphere_attenuation_rate = '0.004'
        dropoff_general_rate = '0.0'
        dropoff_intensity_limit = '0.0'
        dropoff_zero_intensity = '0.0'
        noise_seed = '0.0'
        noise_stddev = '0.0'
        lidar_bp_list = [lidar_bp_2_1, lidar_bp_2_2, lidar_bp_2_3, lidar_bp_4_1, lidar_bp_4_2, lidar_bp_4_3,
                         lidar_bp_6_1,
                         lidar_bp_6_2, lidar_bp_6_3, lidar_bp_28]
        for lidar in lidar_bp_list:
            lidar.set_attribute('range', '200')
            lidar.set_attribute('rotation_frequency', str(int(1 / self.settings.fixed_delta_seconds)))
            lidar.set_attribute('atmosphere_attenuation_rate', atmosphere_attenuation_rate)
            lidar.set_attribute('dropoff_general_rate', dropoff_general_rate)
            lidar.set_attribute('dropoff_intensity_limit', dropoff_intensity_limit)
            lidar.set_attribute('dropoff_zero_intensity', dropoff_zero_intensity)
            lidar.set_attribute('noise_seed', noise_seed)
            lidar.set_attribute('noise_stddev', noise_stddev)
        # 单独设置各雷达参数
        lidar_bp_2_1.set_attribute('channels', '2')
        lidar_bp_2_1.set_attribute('upper_fov', '-1.5')
        lidar_bp_2_1.set_attribute('lower_fov', '-1.7')
        lidar_bp_2_1.set_attribute('points_per_second', '36000')
        lidar_bp_28.set_attribute('channels', '28')
        lidar_bp_28.set_attribute('upper_fov', '-1.9')
        lidar_bp_28.set_attribute('lower_fov', '-4.6')
        lidar_bp_28.set_attribute('points_per_second', '504000')
        lidar_bp_6_1.set_attribute('channels', '6')
        lidar_bp_6_1.set_attribute('upper_fov', '-4.8')
        lidar_bp_6_1.set_attribute('lower_fov', '-5.8')
        lidar_bp_6_1.set_attribute('points_per_second', '108000')
        lidar_bp_4_1.set_attribute('channels', '4')
        lidar_bp_4_1.set_attribute('upper_fov', '-6.1')
        lidar_bp_4_1.set_attribute('lower_fov', '-7')
        lidar_bp_4_1.set_attribute('points_per_second', '72000')
        lidar_bp_4_2.set_attribute('channels', '4')
        lidar_bp_4_2.set_attribute('upper_fov', '-7.4')
        lidar_bp_4_2.set_attribute('lower_fov', '-8.6')
        lidar_bp_4_2.set_attribute('points_per_second', '72000')
        lidar_bp_4_3.set_attribute('channels', '4')
        lidar_bp_4_3.set_attribute('upper_fov', '-9.2')
        lidar_bp_4_3.set_attribute('lower_fov', '-11')
        lidar_bp_4_3.set_attribute('points_per_second', '72000')
        lidar_bp_6_2.set_attribute('channels', '6')
        lidar_bp_6_2.set_attribute('upper_fov', '-12')
        lidar_bp_6_2.set_attribute('lower_fov', '-17')
        lidar_bp_6_2.set_attribute('points_per_second', '108000')
        lidar_bp_6_3.set_attribute('channels', '6')
        lidar_bp_6_3.set_attribute('upper_fov', '-19')
        lidar_bp_6_3.set_attribute('lower_fov', '-29')
        lidar_bp_6_3.set_attribute('points_per_second', '108000')
        lidar_bp_2_2.set_attribute('channels', '2')
        lidar_bp_2_2.set_attribute('upper_fov', '-32')
        lidar_bp_2_2.set_attribute('lower_fov', '-35')
        lidar_bp_2_2.set_attribute('points_per_second', '36000')
        lidar_bp_2_3.set_attribute('channels', '2')
        lidar_bp_2_3.set_attribute('upper_fov', '-38')
        lidar_bp_2_3.set_attribute('lower_fov', '-42')
        lidar_bp_2_3.set_attribute('points_per_second', '36000')
        # 将雷达绑定到主车上
        lidar_2_1 = self.world.spawn_actor(lidar_bp_2_1, transform, attach_to=carla_car)
        lidar_2_2 = self.world.spawn_actor(lidar_bp_2_2, transform, attach_to=carla_car)
        lidar_2_3 = self.world.spawn_actor(lidar_bp_2_3, transform, attach_to=carla_car)
        lidar_4_1 = self.world.spawn_actor(lidar_bp_4_1, transform, attach_to=carla_car)
        lidar_4_2 = self.world.spawn_actor(lidar_bp_4_2, transform, attach_to=carla_car)
        lidar_4_3 = self.world.spawn_actor(lidar_bp_4_3, transform, attach_to=carla_car)
        lidar_6_1 = self.world.spawn_actor(lidar_bp_6_1, transform, attach_to=carla_car)
        lidar_6_2 = self.world.spawn_actor(lidar_bp_6_2, transform, attach_to=carla_car)
        lidar_6_3 = self.world.spawn_actor(lidar_bp_6_3, transform, attach_to=carla_car)
        lidar_28 = self.world.spawn_actor(lidar_bp_28, transform, attach_to=carla_car)
        # 监听雷达数据，并存储到队列中
        lidar_2_1.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_2_1"))
        lidar_2_2.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_2_2"))
        lidar_2_3.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_2_3"))
        lidar_4_1.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_4_1"))
        lidar_4_2.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_4_2"))
        lidar_4_3.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_4_3"))
        lidar_6_1.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_6_1"))
        lidar_6_2.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_6_2"))
        lidar_6_3.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_6_3"))
        lidar_28.listen(lambda data: self.lidar_callback(data, point_cloud_queue, "lidar_28"))
        # 将绑定后的雷达放入集合中
        lidar_list.append(lidar_2_1)
        lidar_list.append(lidar_2_2)
        lidar_list.append(lidar_2_3)
        lidar_list.append(lidar_4_1)
        lidar_list.append(lidar_4_2)
        lidar_list.append(lidar_4_3)
        lidar_list.append(lidar_6_1)
        lidar_list.append(lidar_6_2)
        lidar_list.append(lidar_6_3)
        lidar_list.append(lidar_28)
